/*
 * @Description: front end 任务管理， 放在类里使代码更清晰
 * @Author: Ren Qian
 * @Date: 2020-02-10 08:38:42
 */
#include "lidar_localization/front_end/front_end_flow.hpp"

#include "glog/logging.h"

#include "lidar_localization/tools/file_manager.hpp"
#include "lidar_localization/global_defination/global_defination.h"

namespace lidar_localization {
// 前端工作流对象构造函数
FrontEndFlow::FrontEndFlow(ros::NodeHandle& nh) {
    // 点云消息订阅指针，节点句柄、话题名称、缓存长度，点云数据包含时间戳(秒数)与PCL点云类型数据
    cloud_sub_ptr_ = std::make_shared<CloudSubscriber>(nh, "/kitti/velo/pointcloud", 100000);
    // imu消息订阅指针，节点句柄、话题名称、缓存长度，imu数据包含线加速度、角加速度、姿势四元数、时间戳以及数据插值方法
    imu_sub_ptr_ = std::make_shared<IMUSubscriber>(nh, "/kitti/oxts/imu", 1000000);
    // 速度消息订阅指针，节点句柄、话题名称、缓存长度，速度数据包含线速度、角速度以及时间戳
    velocity_sub_ptr_ = std::make_shared<VelocitySubscriber>(nh, "/kitti/oxts/gps/vel", 1000000);
    // gnss订阅消息指针，gnss数据包括经纬度、高度、局部xyz坐标、时间戳，初始化局部坐标原点方法、gnss坐标转换为局部坐标方法以及数据插值方法
    gnss_sub_ptr_ = std::make_shared<GNSSSubscriber>(nh, "/kitti/oxts/gps/fix", 1000000);
    // tf订阅消息指针，节点句柄、坐标系名称，包换lookData方法获取两个坐标系之间最新的变换矩阵
    lidar_to_imu_ptr_ = std::make_shared<TFListener>(nh, "imu_link", "velo_link");

    // 点云消息发布指针，节点句柄、话题名称、缓冲区长度、坐标系名称，点云消息发布方法，发布当前帧点云消息
    cloud_pub_ptr_ = std::make_shared<CloudPublisher>(nh, "current_scan", 100, "/map");
    // 发布局部地图点云消息
    local_map_pub_ptr_ = std::make_shared<CloudPublisher>(nh, "local_map", 100, "/map");
    // 发布全局地图点云消息
    global_map_pub_ptr_ = std::make_shared<CloudPublisher>(nh, "global_map", 100, "/map");
    // 激光里程计消息发布指针，节点句柄、话题名称、目标坐标系名称、源坐标系名称、缓冲区长度，里程计消息发布方法，发布里程计消息
    laser_odom_pub_ptr_ = std::make_shared<OdometryPublisher>(nh, "laser_odom", "map", "lidar", 100);
    // gnss里程计消息发布指针，话题名称:gnss 目标坐标系名称：map 源坐标系名称：lidar
    gnss_pub_ptr_ = std::make_shared<OdometryPublisher>(nh, "gnss", "map", "lidar", 100);
    
    // 前端指针
    front_end_ptr_ = std::make_shared<FrontEnd>();

    // 点云地图初始化
    local_map_ptr_.reset(new CloudData::CLOUD());
    global_map_ptr_.reset(new CloudData::CLOUD());
    current_scan_ptr_.reset(new CloudData::CLOUD());
}

bool FrontEndFlow::Run() {
    // 从各自消息订阅器队列中读取消息到前端队列
    if (!ReadData())
        return false;
    // 初始化lidar到imu坐标系的变换矩阵
    if (!InitCalibration()) 
        return false;
    // gnss第一条消息的位置作为坐标原点
    if (!InitGNSS())
        return false;
    // 如果前端队列容器都有消息
    while(HasData()) {
        // 如果数据无效重新循环
        if (!ValidData())
            continue;
        // 更新gnss里程计，lidar->imu->map
        UpdateGNSSOdometry();
        // 如果更新激光里程计位姿成功
        if (UpdateLaserOdometry()) {
            // 发布gnss与laser里程计计算的位姿消息以及当前帧与局部地图要展示的点云消息
            PublishData();
            // 保存gnss与laser里程计的轨迹
            SaveTrajectory();
        }
    }

    return true;
}

// 从各自订阅器队列中取消息到前端队列中
bool FrontEndFlow::ReadData() {
    // 点云消息订阅指针从队列容器缓存取消息(实际上已经从点云消息类型转换成pcl点云类型)
    cloud_sub_ptr_->ParseData(cloud_data_buff_);

    // 静态局部变量
    static std::deque<IMUData> unsynced_imu_;
    static std::deque<VelocityData> unsynced_velocity_;
    static std::deque<GNSSData> unsynced_gnss_;

    // 从队列容器缓存读取imu、速度与gnss数据
    imu_sub_ptr_->ParseData(unsynced_imu_);
    velocity_sub_ptr_->ParseData(unsynced_velocity_);
    gnss_sub_ptr_->ParseData(unsynced_gnss_);

    // 如果前端点云队列为空，返回false
    if (cloud_data_buff_.size() == 0)
        return false;

    //否则，根据点云消息时间，拟合出对应的imu、速度以及gnss时间
    double cloud_time = cloud_data_buff_.front().time;
    bool valid_imu = IMUData::SyncData(unsynced_imu_, imu_data_buff_, cloud_time);
    bool valid_velocity = VelocityData::SyncData(unsynced_velocity_, velocity_data_buff_, cloud_time);
    bool valid_gnss = GNSSData::SyncData(unsynced_gnss_, gnss_data_buff_, cloud_time);

    static bool sensor_inited = false; 
    // 如果传感器未初始化
    if (!sensor_inited) {
        // 判断是否找到imu 速度 gnss同步数据
        if (!valid_imu || !valid_velocity || !valid_gnss) {
            cloud_data_buff_.pop_front();
            return false;
        }
        // 都找到则传感器初始化完成
        sensor_inited = true;
    }

    return true;
}

// 初始化lidar到imu坐标系的变换矩阵
bool FrontEndFlow::InitCalibration() {
    static bool calibration_received = false;
    if (!calibration_received) {
        if (lidar_to_imu_ptr_->LookupData(lidar_to_imu_)) {
            calibration_received = true;
        }
    }

    return calibration_received;
}

// 根据第一条gnss数据初始化坐标系位置
bool FrontEndFlow::InitGNSS() {
    static bool gnss_inited = false;
    if (!gnss_inited) {
        GNSSData gnss_data = gnss_data_buff_.front();
        gnss_data.InitOriginPosition();
        gnss_inited = true;
    }

    return gnss_inited;
}

// 判断前端队列容器中是否都有消息
bool FrontEndFlow::HasData() {
    if (cloud_data_buff_.size() == 0)
        return false;
    if (imu_data_buff_.size() == 0)
        return false;
    if (velocity_data_buff_.size() == 0)
        return false;
    if (gnss_data_buff_.size() == 0)
        return false;
    
    return true;
}

bool FrontEndFlow::ValidData() {
    // 取出前端各消息队列的第一条消息
    current_cloud_data_ = cloud_data_buff_.front();
    current_imu_data_ = imu_data_buff_.front();
    current_velocity_data_ = velocity_data_buff_.front();
    current_gnss_data_ = gnss_data_buff_.front();

    // 计算点云消息与imu消息的时间间隔
    double d_time = current_cloud_data_.time - current_imu_data_.time;
    // 如果点云消息太旧，弹出点云消息
    if (d_time < -0.05) {
        cloud_data_buff_.pop_front();
        return false;
    }
    // 如果imu消息太旧，弹出imu 速度 gnss消息
    if (d_time > 0.05) {
        imu_data_buff_.pop_front();
        velocity_data_buff_.pop_front();
        gnss_data_buff_.pop_front();
        return false;
    }

    cloud_data_buff_.pop_front();
    imu_data_buff_.pop_front();
    velocity_data_buff_.pop_front();
    gnss_data_buff_.pop_front();

    return true;
}

bool FrontEndFlow::UpdateGNSSOdometry() {
    gnss_odometry_ = Eigen::Matrix4f::Identity();
    // 收到的gnss全局坐标信息转换成局部坐标系的坐标
    current_gnss_data_.UpdateXYZ();
    // gnss里程计的平移部分
    gnss_odometry_(0,3) = current_gnss_data_.local_E;
    gnss_odometry_(1,3) = current_gnss_data_.local_N;
    gnss_odometry_(2,3) = current_gnss_data_.local_U;
    // gnss里程计的旋转部分由imu提供
    gnss_odometry_.block<3,3>(0,0) = current_imu_data_.GetOrientationMatrix();
    // lidar->imu->map
    gnss_odometry_ *= lidar_to_imu_;

    return true;
}

bool FrontEndFlow::UpdateLaserOdometry() {
    static bool front_end_pose_inited = false;
    if (!front_end_pose_inited) {
        front_end_pose_inited = true;
        // 前端未初始化位姿时，用gnss里程计位姿作为前端初始化位姿
        front_end_ptr_->SetInitPose(gnss_odometry_);
        // 根据当前点云数据更新雷达里程计位姿
        return front_end_ptr_->Update(current_cloud_data_, laser_odometry_);
    }
    // 把上次计算得到的激光里程计位姿重新初始化为单位阵
    laser_odometry_ = Eigen::Matrix4f::Identity();
    // 使用当前点云数据与局部地图配准，返回激光里程计配准的位姿，即当前点云数据当地图的变换矩阵
    return front_end_ptr_->Update(current_cloud_data_, laser_odometry_);
}

bool FrontEndFlow::PublishData() {
    // gnss_odometry是lidar到map的变换矩阵，但是发布的消息是位置和四元数
    gnss_pub_ptr_->Publish(gnss_odometry_);
    // laser_odometry同上
    laser_odom_pub_ptr_->Publish(laser_odometry_);

    // 对配准之后的当前帧进行滤波展示
    front_end_ptr_->GetCurrentScan(current_scan_ptr_);
    // 点云数据转换成ROS点云消息，加时间戳与坐标系名称发布
    cloud_pub_ptr_->Publish(current_scan_ptr_);
    // 如果局部地图更新并对其滤波后，同上发布点云数据
    if (front_end_ptr_->GetNewLocalMap(local_map_ptr_))
        local_map_pub_ptr_->Publish(local_map_ptr_);

    return true;
}

bool FrontEndFlow::SaveTrajectory() {
    static std::ofstream ground_truth, laser_odom;
    static bool is_file_created = false;
    if (!is_file_created) {
        // 创建轨迹目录
        if (!FileManager::CreateDirectory(WORK_SPACE_PATH + "/slam_data/trajectory"))
            return false;
        // 以app模式打开ground_truth轨迹文件
        if (!FileManager::CreateFile(ground_truth, WORK_SPACE_PATH + "/slam_data/trajectory/ground_truth.txt"))
            return false;
        // 以app模式打开laser_odom轨迹文件
        if (!FileManager::CreateFile(laser_odom, WORK_SPACE_PATH + "/slam_data/trajectory/laser_odom.txt"))
            return false;
        is_file_created = true;
    }

    // 把变换矩阵的前三行数据直接保存
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 4; ++j) {
            ground_truth << gnss_odometry_(i, j);
            laser_odom << laser_odometry_(i, j);
            if (i == 2 && j == 3) {
                ground_truth << std::endl;
                laser_odom << std::endl;
            } else {
                ground_truth << " ";
                laser_odom << " ";
            }
        }
    }

    return true;
}

bool FrontEndFlow::SaveMap() {
    return front_end_ptr_->SaveMap();
}

bool FrontEndFlow::PublishGlobalMap() {
    // 对新得到的全局地图进行滤波
    if (front_end_ptr_->GetNewGlobalMap(global_map_ptr_)) { 
        // 如果存在新地图并且滤波成功后使用全局地图发布指针对其发布
        global_map_pub_ptr_->Publish(global_map_ptr_);
        global_map_ptr_.reset(new CloudData::CLOUD());
    }
    return true;
}
}